from lerobot.common.robot_devices.control_configs import ControlPipelineConfig
from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot
import draccus
import torch

cli_args_l = ['--robot.type=so101L', '--robot.cameras={}', '--control.type=teleoperate']
cli_args_f = ['--robot.type=so101F', '--robot.cameras={}', '--control.type=teleoperate']

cfg_l = draccus.parse(config_class=ControlPipelineConfig, config_path=None, args=cli_args_l)
cfg_f = draccus.parse(config_class=ControlPipelineConfig, config_path=None, args=cli_args_f)

robotl = ManipulatorRobot(cfg_l.robot)

robotf = ManipulatorRobot(cfg_f.robot)

robotl.connect()
robotf.connect()


while 1:
    leader_pos = {}
    for name in robotl.leader_arms:
        leader_pos[name] = robotl.leader_arms[name].read("Present_Position")
        leader_pos[name] = torch.from_numpy(leader_pos[name])
    print(leader_pos)

    robotf.send_action(leader_pos['main'])



breakpoint()

s = 1